#!/usr/bin/env python
"""
Basic layout of robot:
		__W2__
     /      \
    /        \e
    \        /
	W1--------W3

This robot is an omni-directional robot, built around a hexagonal shape
Each of the wheels are 135 degrees apart from one another.
Rotation in the clockwise direction is viewed as forward motion.
With rotation in the counter-clockwise direction is viewed as reverse motion

"""

import socket
import sys
import array
import struct
import time
import serial

ser = serial.Serial()
ser.port = "/dev/ttyACM0"
ser.baudrate = 9600
ser.timeout = 0
ser.open()

##Global variables
byte0 = int(0xFF)
byte1 = int(0)

m1 = int(0)
m2 = int(0)
m3 = int(0)

## IDs for the motor drivers, these should not change
id3 = int(0x0D)
id2 = int(0x1E)
id1 = int(0x19)

## hex values for forward and reverse
fwd = int(5)
rev = int(6)

## variables for motor directions
dir1 = fwd
dir2 = fwd
dir3 = fwd

## Socket Object in python
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)

def hundred():
	global m1, m2, m3, dir1, dir2, dir3
	m1 = int(90)
	m2 = int(0)
	m3 = int(90)
	dir1 = fwd
	dir3 = rev

	packet_int = struct.pack('BBBBBBBBBBB', byte0, byte1, id1, dir1, m1, id2, dir2, m2, id3, dir3, m3)
	s.send(packet_int)


def motor1():
	global m1, m2, m3, dir1, dir2, dir3
	if (m1 <= 0):
		m1 = int(15)
	elif (m1 <= int(100)):
		m1 += int(5)
	m2 = int(0)
	m3 = int(0)
	dir1 = fwd

	packet_int = struct.pack('BBBBBBBBBBB', byte0, byte1, id1, dir1, m1, id2, dir2, m2, id3, dir3, m3)
	s.send(packet_int)

def motor2():
	global m1, m2, m3, dir1, dir2, dir3
	if (m2 <= 0):
		m2 = int(15)
	elif (m2 <= int(100)):
		m2 += int(5)
	m1 = int(0)
	m3 = int(0)
	dir2 = fwd

	packet_int = struct.pack('BBBBBBBBBBB', byte0, byte1, id1, dir1, m1, id2, dir2, m2, id3, dir3, m3)
	s.send(packet_int)

def motor3():
	global m1, m2, m3, dir1, dir2, dir3
	if (m3 <= 0):
		m3 = int(15)
	elif (m3 <= int(100)):
		m3 += int(5)
	m1 = int(0)
	m2 = int(0)
	dir3 = fwd

	packet_int = struct.pack('BBBBBBBBBBB', byte0, byte1, id1, dir1, m1, id2, dir2, m2, id3, dir3, m3)
	s.send(packet_int)

def irStuff():
	irByte0 = int(0xaa)
	irByte1 = int(0x00)
	irByte2 = int(0xff)
	irByte3 = int(0x00)

	packet_int = struct.pack('BBBBBBBBBBB', irByte0, irByte1, irByte2, irByte3, irByte3, irByte3, irByte3, irByte3, irByte3, irByte3, irByte3)
	s.send(packet_int)

## Connects to the server
def connectSvr():
	host = '192.168.1.138'
	port = 50000
	size = 100
	
	try:
		s.connect((host,port))
		print '\nConnecting to Server...\n'
	except Exception, e:
		alert('Something is wrong with %s:%d. Exception is type %s' %(host, port, e))
	print 'Connection Established!!!\n'
## End of Connect Svr

# prints and identifies exception type for connection issues
def alert(msg):
	print >>sys.stderr, msg
	sys.exit(1)

## this is for angle 0
def wButton():
	global m1, m2, m3, dir1, dir2, dir3
	## If the robot is currently going in the 180 angle direction
	if ((dir1 == rev) and (dir3 == fwd)):
		## and while the velocity is greater than 0
		if ((m1 >= 5) and (m3 >= 5)):
			## Then decriment that velocity
			m1 -= int(5)
			m3 -= int(5)
			dir1 = rev
			dir2 = fwd
			dir3 = fwd	
		## Once the velocity reaches 0 then change the direction
		elif ((m1 <= 0) and (m2 <= 0)):
			m1 = int(0)
			m3 = int(0)
			dir1 = fwd
			dir3 = rev
	else: ## This is the default case for going forward
		if ((m1 == 0) and (m3 == 0)):
			m1 = int(15)
			m3 = int(15)
		elif ((m1 <= int(100)) and (m3 <= 100)):
			m1 += int(5)
			m2 = int(0)
			m3 += int(5)
		dir1 = fwd
		dir2 = fwd
		dir3 = rev

	packet_int = struct.pack('BBBBBBBBBBB', byte0, byte1, id1, dir1, m1, id2, dir2, m2, id3, dir3, m3)
	s.send(packet_int)

## this is for angle 180
def sButton():
	global m1, m2, m3, dir1, dir2, dir3
	## If the robot is currently moving in the 0 degree direction
	## then this function will decrease speed
	if ((dir1 == fwd) and (dir3 == rev)):
		if ((m1 >= 5) and (m3 >= 5)):
			m1 -= int(5)
			m2 = int(0)
			m3 -= int(5)
			dir1 = fwd
			dir2 = fwd
			dir3 = rev
		## Once m1 and m3 reach 0 then the motors change directions
		elif ((m1 <= 0) and (m3 <= 0)):
			m1 = int(0)
			m3 = int(0)
			dir1 = rev
			dir3 = fwd
	## else once it reaches a stop it will
	## change directions and start moving in reverse
	#	elif ((m1 == 0) and (m2 == 0)):
	else:	## This is the default case for going backwards
		if ((m1 == 0) and (m3 == 0)):
			m1 = int(15)
			m3 = int(15)
		else:
			m1 += int(5)
			m3 += int(5)
		dir1 = rev
		dir2 = fwd
	packet_int = struct.pack('BBBBBBBBBBB', byte0, byte1, id1, dir1, m1, id2, dir2, m2, id3, dir3, m3)
	s.send(packet_int)

## this is for angle 45
def eButton():
	global m1, m2, m3, dir1, dir2, dir3
	## If the robot is currently going in the -135 direction
	if ((dir2 == rev) and (dir3 == fwd)):
		## and while the velocity is greater than 0
		if ((m2 >= 5) and (m3 >= 5)):
			## Then decriment that velocity
			m2 -= int(5)
			m3 -= int(5)
			dir1 = rev
			dir2 = rev
			dir3 = fwd	
		## Once the velocity reaches 0 then change the direction
		elif ((m2 <= 0) and (m3 <= 0)):
			m1 = int(0)
			m2 = int(0)
			m3 = int(0)
			dir2 = fwd
			dir3 = rev
	else: ## This is the default case for going forward
		if ((m2 == 0) and (m3 == 0)):
			m2= int(15)
			m3 = int(15)
		elif ((m1 <= int(100)) and (m3 <= int(100))):
			m1 = int(0)
			m2 += int(5)
			m3 += int(5)
		dir1 = fwd
		dir2 = fwd
		dir3 = rev

	packet_int = struct.pack('BBBBBBBBBBB', byte0, byte1, id1, dir1, m1, id2, dir2, m2, id3, dir3, m3)
	s.send(packet_int)
## End of 45 degree


## This is for angle 90, this one is tricky
def dButton():
	global m1, m2, m3, dir1, dir2, dir3
	"""if ((dir1 == fwd) and (dir2 == rev) and (dir3 == rev)):
		if ((m1 >= 6) and (m2 >= 3) and (m3 >= 6)):
			m1 -= int(6)
			m2 -= int(3)
			m3 -= int(6)
		else:
			m1 = int(0)
			m2 = int(0)
			m3 = int(0)
			dir1 = rev
dir2 = fwd
dir3 = fwd
else:
if ((m1 == 0) or (m2 == 0) or (m3 == 0)):
m1 = int(23)
m2 = int(15)
m3 = int(23)
elif ((m1 <= int(100)) and (m2 <= int(100)) and (m3 <= int(100))):
m1 += int(6)
m2 += int(3)
m3 += int(6)
dir1 = rev
dir2 = fwd
dir3 = rev	"""
	m1 = int(15)
	m2 = int(30)
	m3 = int(15)
	dir1 = rev
	dir2 = fwd
	dir3 = rev
	packet_int = struct.pack('BBBBBBBBBBB', byte0, byte1, id1, dir1, m1, id2, dir2, m2, id3, dir3, m3)
	s.send(packet_int)
## End of angle 90

## This is for movement for angle -90
def aButton():
	global m1, m2, m3, dir1, dir2, dir3
	m1 = int(15)
	m2 = int(30)
	m3 = int(15)
	dir1 = fwd
	dir2 = rev
	dir3 = fwd
	packet_int = struct.pack('BBBBBBBBBBB', byte0, byte1, id1, dir1, m1, id2, dir2, m2, id3, dir3, m3)
	s.send(packet_int)
## End of angle -90

## This is for counte-clockwise rotation
def ccwButton():
	global m1, m2, m3
	m1 = m2 = m3 = int(20)
	dir1 = dir2 = dir3 = rev
	
	packet_int = struct.pack('BBBBBBBBBBB', byte0, byte1, id1, dir1, m1, id2, dir2, m2, id3, dir3, m3)
	s.send(packet_int)

## This is for clockwise rotation
def cwButton():
	global m1, m2, m3
	m1 = m2 = m3 = int(20)
	dir1 = dir2 = dir3 = fwd	

	packet_int = struct.pack('BBBBBBBBBBB', byte0, byte1, id1, dir1, m1, id2, dir2, m2, id3, dir3, m3)
	s.send(packet_int)

"""## This is for angle 45
def eButton():
	global m1, m2, m3, dir1, dir2, dir3
	## If the robot is currently going in the reverse direction
	if ((dir2 == rev) and (dir3 == fwd)):
		## and while the velocity is greater than 0
		if ((m2 > 0) and (m3 > 0)):
			## Then decriment that velocity
			m2 -= int(5)
			m3 -= int(5)
			dir1 = fwd
			dir2 = fwd
			dir3 = rev	
		## Once the velocity reaches 0 then change the direction
		elif ((m2 <= 0) and (m3 <= 0)):
			m2 = int(0)
			m3 = int(0)
			dir2 = fwd
			dir3 = rev
	else: ## This is the default case for going forward
		if ((m2 == 0) and (m3 == 0)):
			m2 = int(15)
			m3 = int(15)
		else:
			m2 += int(5)
			m1 = int(0)
			m3 += int(5)
		dir1 = fwd
		dir2 = fwd
		dir3 = rev

	packet_int = struct.pack('BBBBBBBBBBB', byte0, byte1, id1, dir1, m1, id2, dir2, m2, id3, dir3, m3)
	s.send(packet_int)
## End of angle 45
"""
## This is for angle 135
def xButton():
	global m1, m2, m3, dir1, dir2, dir3
	## If the robot is currently going in the reverse direction
	if ((dir1 == fwd) and (dir2 == rev)):
		## and while the velocity is greater than 0
		if ((m1 > 0) and (m2 > 0)):
			## Then decriment that velocity
			m1 -= int(5)
			m3 -= int(5)
			m2 = int(0)
			dir1 = fwd
			dir2 = rev
			dir3 = fwd	
		## Once the velocity reaches 0 then change the direction
		elif ((m1 <= 0) and (m2 <= 0)):
			m1 = int(0)
			m2 = int(0)
			m3 = int(0)
			dir1 = rev
			dir2 = fwd
	else: ## This is the default case for going forward
		if ((m1 == 0) and (m2 == 0)):
			m1 = int(15)
			m2 = int(15)
		else:
			m1 += int(5)
			m3 = int(0)
			m2 += int(5)
		dir1 = rev
		dir2 = fwd

	packet_int = struct.pack('BBBBBBBBBBB', byte0, byte1, id1, dir1, m1, id2, dir2, m2, id3, dir3, m3)
	s.send(packet_int)
## End of angle 135

## This is for angle -135
def zButton():
	global m1, m2, m3, dir1, dir2, dir3
	## If the robot is currently going in the E direction
	if ((dir2 == fwd) and (dir3 == rev)):
		## and while the velocity is greater than 0
		if ((m2 > 0) and (m3 > 0)):
			## Then decriment that velocity
			m1 = int(0)
			m2 -= int(5)
			m3 = int(5)
			dir1 = fwd
			dir2 = fwd
			dir3 = rev	
		## Once the velocity reaches 0 then change the direction
		elif ((m2 <= 0) and (m3 <= 0)):
			m1 = int(0)
			m2 = int(0)
			m3 = int(0)
			dir2 = rev
			dir3 = fwd
	else: ## This is the default case for going forward
		if ((m2 == 0) and (m3 == 0)):
			m2 = int(15)
			m3 = int(15)
		else:
			m1 = int(0)
			m2 += int(5)
			m3 += int(5)
		dir2 = rev
		dir3 = fwd

	packet_int = struct.pack('BBBBBBBBBBB', byte0, byte1, id1, dir1, m1, id2, dir2, m2, id3, dir3, m3)
	s.send(packet_int)
## End of angle -135

## Movement for angle -45
def qButton():
	global m1, m2, m3, dir1, dir2, dir3
	## If the robot is currently going in the -135 degree direction
	if ((dir1 == rev) and (dir2 == fwd)):
		## and while the velocity is greater than 0
		if ((m1 > 0) and (m2 > 0)):
			## Then decriment that velocity
			m1 -= int(5)
			m2 -= int(5)
			m3 = int(0)
			dir1 = rev
			dir2 = fwd
			dir3 = fwd	
		## Once the velocity reaches 0 then change the direction
		elif ((m1 <= 0) and (m2 <= 0)):
			m1 = int(0)
			m2 = int(0)
			m3 = int(0)
			dir1 = fwd
			dir2 = rev
	else: ## This is the default case for going forward
		if ((m1 == 0) and (m2 == 0)):
			m1 = int(15)
			m2 = int(15)
		else:
			m1 += int(5)
			m3 = int(0)
			m2 += int(5)
		dir1 = fwd
		dir2 = rev

	packet_int = struct.pack('BBBBBBBBBBB', byte0, byte1, id1, dir1, m1, id2, dir2, m2, id3, dir3, m3)
	s.send(packet_int)
## end of angle -45

def stp():
	global m1, m2, m3
	m1 = int(0)
	m2 = int(0)
	m3 = int(0)
	dir1 = fwd
	dir2 = fwd
	dir3 = fwd
	
	packet_int = struct.pack('BBBBBBBBBBB', byte0, byte1, id1, dir1, m1, id2, dir2, m2, id3, dir3, m3)
	s.send(packet_int)

def main():
	running = 1
	while running:
		nes = ser.read(1)
		if len(nes) > 0:
			uInput = nes
	##	uInput = raw_input('Junk and stuff\n')
		## angle 0
			if uInput == 'w':
				wButton()
			## angle 180
			elif uInput == 's':
				sButton()
			## Counter-Clockwise Rotation
			elif uInput == ',':
				ccwButton()
			## Clockwise rotation
			elif uInput == '.':
				cwButton()
			## angle 90
			elif uInput == 'd':
				dButton()
			## angle 45
			elif uInput == 'e':
				eButton()
			## angle 135
			elif uInput == 'x':
				xButton()
			## angle -135
			elif uInput == 'z':
				zButton()
			## angle -90
			elif uInput == 'a':
				aButton()
			## Angle -45
			elif uInput == 'q':
				qButton()
			elif uInput == 'ir':
				irStuff()
			elif uInput == 'm1':
				motor1()
			elif uInput == 'm2':
				motor2()
			elif uInput == 'm3':
				motor3()
			elif uInput == 'f':
				hundred()	
			## Stop motors
			elif uInput == 'h':
				stp()

connectSvr()
main()


